/*
 * Copyright(C),2024-2024, HUTT.Co.Ltd.
 * Author : yangxiaosheng
 * date: 2024-11-13 15:55:24
 * File   : pidAlgo.c
 * History:
 */

#include "pidAlgo.h"

#include <float.h>

#define PRINT_ERR_RET_VOID(cond, fmt, ...)                                     \
    do {                                                                       \
        if (cond) {                                                            \
            printf("ERROR:[%s](%d)\t" fmt, __func__, __LINE__, ##__VA_ARGS__); \
            return;                                                            \
        };                                                                     \
    } while (0)
#define LIMIT_MAX_MIN(x, max, min) ((x) > (max) ? (max) : ((x) < (min) ? (min) : (x)))

/* pid数据清除 */
void pidDataClr(pidType *pid)
{
    PRINT_ERR_RET_VOID(!pid, "input para is NULL\r\n");

    pid->integral = 0;
    pid->errPre = 0;
    pid->errPrePre = 0;
    pid->ukInc = 0;
    pid->ukPos = 0;
}

/* pid初始化 */
void pidInit(double kp, double ki, double kd, pidType *pid)
{
    PRINT_ERR_RET_VOID(!pid, "input para is NULL\r\n");
    // PRINT_ERR_RET_VOID(kp < 1e-18 || kd < 1e-18, "Invalid PID parameters\r\n");

    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->integral = 0;
    pid->errPre = 0;
    pid->errPrePre = 0;
    pid->ukInc = 0;
    pid->ukPos = 0;
}

/* 位置式pid
 入参：pos_pid.actVal = test[i];
 出参：pid->ukPos
*/
void positionPid(pidType *pid)
{
    PRINT_ERR_RET_VOID(!pid, "input para is NULL\r\n");
    pid->errPre = pid->err;
    pid->err = pid->target - pid->actVal;
#if 1
    /* 改进的抗积分饱和策略 */
    if (pid->ki > 1e-15 || pid->ki < -1e-15) {
        double integralTemp = pid->integral + pid->err;
        if (integralTemp > pid->max / pid->ki) {
            integralTemp = pid->max / pid->ki;
        } else if (integralTemp < pid->min / pid->ki) {
            integralTemp = pid->min / pid->ki;
        }
        pid->integral = integralTemp;
    }
#endif

    pid->ukPos = pid->kp * pid->err + pid->ki * pid->integral + pid->kd * (pid->err - pid->errPre);
    pid->ukPos = LIMIT_MAX_MIN(pid->ukPos, pid->max, pid->min);
}

/* 增量式PID，真正输出 pid->ukInc
 入参：inc_pid.actVal = test[i];
 出参：pid->ukInc
*/
void incrementalPid(pidType *pid)
{
    PRINT_ERR_RET_VOID(!pid, "input para is NULL\r\n");

    pid->errPre = pid->err;
    pid->err = pid->target - pid->actVal;

    float inc = pid->kp * (pid->err - pid->errPre) +
                pid->ki * pid->err +
                pid->kd * (pid->err - 2 * pid->errPre + pid->errPrePre); /*pid计算得到的增量*/
    pid->errPrePre = pid->errPre;
    pid->ukInc += inc;
    pid->ukInc = LIMIT_MAX_MIN(pid->ukInc, pid->max, pid->min);
}

void pidSetTarget(pidType *pid, float setVal)
{
    PRINT_ERR_RET_VOID(!pid, "input NULL\r\n");
    pid->target = setVal;
}

/* 真实值，传感器值 */
void pidSetActVal(pidType *pid, float setVal)
{
    PRINT_ERR_RET_VOID(!pid, "input NULL\r\n");
    pid->actVal = setVal;
}

void pidSetUkMaxMin(pidType *pid, int max, int min)
{
    PRINT_ERR_RET_VOID(!pid, "input NULL\r\n");
    pid->max = max;
    pid->min = min;
}